#include "Laserdis.h"
//#include "bsp_delay.h"
#include "usart.h"
//#include "led.h"

void Laser_Addr_Set(uint8_t addr)
{
	uint8_t buf[10] = {0},sum = 0;
	buf[0] = 0XFA; 
	buf[1] = 0X04;
	buf[2] = 0X01;
	buf[3] = addr;
	sum = buf[0] + buf[1] + buf[2] + buf[3];
	buf[4] = ~sum+1;
	RS485_Send_Data(buf,5);
}

void Laser_ReadCmd_Send(uint8_t cmd)
{
	uint8_t buf[10] = {0XFA,0X06,0X01,0XFF};
	if(cmd==READ_DATA){
		buf[0] = 0XFA; buf[1] = 0X06;buf[2] = 0X01;buf[3] = 0XFF;
		RS485_Send_Data(buf,4);
	}
	else if(cmd==READ_DEVICE_NUM){
		buf[0] = 0XFA; buf[1] = 0X06;buf[2] = 0X04;buf[3] = 0XFC;
		RS485_Send_Data(buf,4);
	}
}

void Laser_Set_Device_Num(uint8_t num)
{
	uint8_t buf[10] = {0},sum = 0;
	buf[0] = 0XFA; 
	buf[1] = 0X04;
	buf[2] = 0X01;
	buf[3] = num;
	sum = buf[0] + buf[1] + buf[2] + buf[3];
	buf[4] = ~sum+1;
	RS485_Send_Data(buf,5);
}

void Laser_Light_Ctr(uint8_t addr,uint8_t mode)
{
	uint8_t buf[10] = {0},sum = 0;
	buf[0] = addr; 
	buf[1] = 0X06;
	buf[2] = 0X05;
	if(mode!=0){
		buf[3] = 0X01;
	}
	else{
		buf[3] = 0X00;
	}
	sum = buf[0] + buf[1] + buf[2] +buf[3];
	buf[4] = ~sum+1;
	RS485_Send_Data(buf,5);
}

void Laser_Set_Range(uint8_t ls)
{
	uint8_t buf[10] = {0},sum = 0;
	buf[0] = 0XFA; 
	buf[1] = 0X04;
	buf[2] = 0X09;
	buf[3] = ls;
	sum = buf[0] + buf[1] + buf[2] +buf[3];
	buf[4] = ~sum+1;
	RS485_Send_Data(buf,5);
}

void Laser_Set_Freq(uint8_t f)
{
	uint8_t buf[10] = {0},sum = 0;
	buf[0] = 0XFA; 
	buf[1] = 0X04;
	buf[2] = 0X0A;
	buf[3] = f;
	sum = buf[0] + buf[1] + buf[2] +buf[3];
	buf[4] = ~sum+1;
	RS485_Send_Data(buf,5);
}

void Laser_Set_Resolution(uint8_t res)
{
	uint8_t buf[10] = {0},sum = 0;
	buf[0] = 0XFA; 
	buf[1] = 0X04;
	buf[2] = 0X0C;
	buf[3] = res;
	sum = buf[0] + buf[1] + buf[2] +buf[3];
	buf[4] = ~sum+1;
	RS485_Send_Data(buf,5);
}

void Laser_Single_Measure(uint8_t addr)
{
	uint8_t buf[10] = {0},sum = 0;
	buf[0] = addr; 
	buf[1] = 0X06;
	buf[2] = 0X02;
	sum = buf[0] + buf[1] + buf[2];
	buf[3] = ~sum+1;
	RS485_Send_Data(buf,4);
}

float Laser_dis1 = 0, Laser_dis2 = 0, Laser_dis3 = 0, Laser_dis4 = 0;
void Laser_Dis_Analys(uint8_t buf[256],float *dis1,float *dis2,float *dis3,float *dis4)
{
	uint8_t temp[64] = {0},i = 0,mark = 0;
	for(i=0;i<64;i++)
	{
		if(buf[i]==DEVICE0||buf[i]==DEVICE1||buf[i]==DEVICE2||buf[i]==DEVICE3)
		{
			if(buf[i+1]==0X06 && buf[i+2]==0X82){
				mark = i;
				for(i=0;(mark+i)<64;i++)
				{
					temp[i] = buf[mark+i];
				}
				if(mark>=54){
					return;
				}
				else{
					break;
				}
			}
		}
	}
	if(buf[mark]==DEVICE0){
		*dis1 = parse_str_to_num(temp,11);
	}
	else if(buf[mark]==DEVICE1){
		*dis2 = parse_str_to_num(temp,11);
	}
	else if(buf[mark]==DEVICE2){
		*dis3 = parse_str_to_num(temp,11);
	}
	else if(buf[mark]==DEVICE3){
		*dis4 = parse_str_to_num(temp,11);
	}
}

double parse_str_to_num(uint8_t buf[],uint8_t len)
{
	uint8_t i = 0,res = 0,mark = 0;
	double weishu[8] = {0},xiaoshu[16] = {0};
	double val = 0;
	
	for(i=0;i<len;i++)
	{
		if(buf[i]=='.'){
			mark = i;
		}
	}
	for(i=0;i<len;i++){
		if(buf[i]>=0X30&&buf[i]<=0X39){
			res = ascii_to_num(buf[i]);
			if(i<mark){
				weishu[mark-i] = res;
			}
			else if(i>mark){
				xiaoshu[i-mark] = res;
			}
		}
	}
	val = weishu[5]*10000+weishu[4]*1000+ weishu[3]*100+weishu[2]*10+ weishu[1]\
	+xiaoshu[1]*0.1f+xiaoshu[2]*0.01f+xiaoshu[3]*0.001f+xiaoshu[4]*0.0001f+xiaoshu[5]*0.00001f+\
	+xiaoshu[6]*0.000001f+xiaoshu[7]*0.0000001f+xiaoshu[8]*0.00000001f+xiaoshu[9]*0.000000001f;
	return val;
}

uint8_t ascii_to_num(uint8_t ascii)
{
	uint8_t res = 0;
	switch(ascii)
	{
		case 0X30:
		res = 0;
		break;
		case 0X31:
		res = 1;
		break;
		case 0X32:
		res = 2;
		break;
		case 0X33:
		res = 3;
		break;
		case 0X34:
		res = 4;
		break;
		case 0X35:
		res = 5;
		break;
		case 0X36:
		res = 6;
		break;
		case 0X37:
		res = 7;
		break;
		case 0X38:
		res = 8;
		break;
		case 0X39:
		res = 9;
		break;
		default:
			return 0;
	}
	return res;
}

